#ifndef IMU_H
#define IMU_H
#include <ICM20948_WE.h>
#include <Arduino.h>
#define PI 3.14159265358979323846
#define DEG_TO_RAD 0.01745329 //角度转弧度常量
#define RAD_TO_DEG 57.29578
typedef struct
{
    float x;
    float y;
    float z;

} Float_xyz;
typedef struct
{
/*
原始数据相关变量
*/
 Float_xyz accRaw;//加速度计原始数据
 Float_xyz accoffset;//加速度计零飘校正数据
 Float_xyz accdata;//加速度计校正后数据
 Float_xyz gyrRaw;//陀螺仪原始数据
 Float_xyz gyroffset;//陀螺仪零飘校正数据
 Float_xyz gyrdata;//陀螺仪校正后数据
 float pitch_acc;//加速度计解算出的pitch角度
 float roll_acc;//加速度计解算出的roll角度
 float pitch_gyro;//陀螺仪积分得到的pitch角度
 float roll_gyro;//陀螺仪积分得到的roll角度
 float yaw_gyro;//陀螺仪积分得到的roll角度
 float pitch;//融合后的欧拉角
 float roll;//融合后的欧拉角
 float yaw;//融合后的欧拉角
 float alpha=0.95;//融合占比参数
/* 
  辅助初始化相关变量
*/
 bool IMU_init_flag=false;//IMU加速度计辅助初始化标志位
 float pitch_setup_data;//pitch积分解算初始角度，由加速度计得出
 float roll_setup_data;//roll积分解算初始角度，由加速度计得出
/*
Z速度解算相关变量
*/
 int delay_speed_time;//延迟开始速度解算时间
 float sin_pitch=0;//欧拉角的三角值，为后续解算加速度计在重力方向上的分量做准备
 float cos_pitch=0;
 float sin_roll=0;
 float cos_roll=0;
 float aircraft_att[3]={0};//初始化一个数组，存放解算需要的三角函数值
 float no_filteraccz=0;//没有滤波的重力方向加速度
 float speed_z=0;
 float high=0;
} IMU_DATA_t;

void offset_input(IMU_DATA_t*imu_data);
void autooffset(IMU_DATA_t*imu_data);
void getaccdata(IMU_DATA_t*imu_data);
void getgyrdata(IMU_DATA_t*imu_data);
void acctask(void *pvParameters);
void gyrtask(void *pvParameters);
void solvetask(void *pvParameters);
void imu_init();
IMU_DATA_t*get_imudata_point(void);
#endif // IMU_H